// 在包含任何ROS2头文件之前先定义_GNU_SOURCE宏
#define _GNU_SOURCE

// 先包含所有系统头文件
#include <fcntl.h>
#include <sys/mman.h>
#include <sys/stat.h>
#include <time.h>
#include <signal.h>
#include <pthread.h>
#include <unistd.h>
#include <cerrno>
#include <cstring>

// 然后包含ROS2头文件
#include <rclcpp/rclcpp.hpp>

// 最后包含自定义头文件和消息
#include "plc_bridge.h"
#include "agv_msgs/msg/agv_event.hpp"
#include "agv_msgs/msg/agv_mode.hpp"
#include "agv_msgs/msg/agv_auto_status.hpp"

using namespace std::chrono_literals;

class PLCBrigde : public rclcpp::Node {
public:
    PLCBrigde() : Node("plc_bridge") {
        // 创建写共享内存 (AGV -> PLC)
        shm_fd_write_ = shm_open("CODESYS_MEMORY_READ", O_CREAT | O_RDWR, 0666);
        if (shm_fd_write_ == -1) {
            RCLCPP_FATAL(this->get_logger(), "shm_open(WRITE) failed: %s", strerror(errno));
            rclcpp::shutdown();
            return;
        }

        // 设置写共享内存大小
        if (ftruncate(shm_fd_write_, sizeof(WriteToPLCStruct)) == -1) {
            RCLCPP_FATAL(this->get_logger(), "ftruncate(WRITE) failed: %s", strerror(errno));
            close(shm_fd_write_);
            rclcpp::shutdown();
            return;
        }

        // 映射写共享内存
        void* write_ptr = mmap(NULL, sizeof(WriteToPLCStruct), 
                              PROT_READ | PROT_WRITE, MAP_SHARED, shm_fd_write_, 0);
        if (write_ptr == MAP_FAILED) {
            RCLCPP_FATAL(this->get_logger(), "mmap(WRITE) failed: %s", strerror(errno));
            close(shm_fd_write_);
            rclcpp::shutdown();
            return;
        }
        shm_struct_write_ = static_cast<WriteToPLCStruct*>(write_ptr);
        
        // 创建读共享内存 (PLC -> AGV)
        shm_fd_read_ = shm_open("CODESYS_MEMORY_WRITE", O_CREAT | O_RDWR, 0666);
        if (shm_fd_read_ == -1) {
            RCLCPP_FATAL(this->get_logger(), "shm_open(READ) failed: %s", strerror(errno));
            rclcpp::shutdown();
            return;
        }

        // 设置读共享内存大小
        if (ftruncate(shm_fd_read_, sizeof(ReadFromPLCStruct)) == -1) {
            RCLCPP_FATAL(this->get_logger(), "ftruncate(READ) failed: %s", strerror(errno));
            close(shm_fd_read_);
            rclcpp::shutdown();
            return;
        }

        // 映射读共享内存
        void* read_ptr = mmap(NULL, sizeof(ReadFromPLCStruct), 
                             PROT_READ | PROT_WRITE, MAP_SHARED, shm_fd_read_, 0);
        if (read_ptr == MAP_FAILED) {
            RCLCPP_FATAL(this->get_logger(), "mmap(READ) failed: %s", strerror(errno));
            close(shm_fd_read_);
            rclcpp::shutdown();
            return;
        }
        shm_struct_read_ = static_cast<ReadFromPLCStruct*>(read_ptr);
        
        RCLCPP_INFO(this->get_logger(), "Dual shared memory initialized");
        InitializeSharedMemory();
        
        // 创建订阅器
        event_sub_ = create_subscription<agv_msgs::msg::AGVEvent>(
            "/eventstatus", 10,
            [this](const agv_msgs::msg::AGVEvent::SharedPtr msg) {
                WriteEventToShm(msg);
            });
        
        mode_sub_ = create_subscription<agv_msgs::msg::AGVMode>(
            "/agvmode", 10,
            [this](const agv_msgs::msg::AGVMode::SharedPtr msg) {
                WriteModeToShm(msg);
            });
        
        status_sub_ = create_subscription<agv_msgs::msg::AgvAutoStatus>(
            "/auto_run_status", 10,
            [this](const agv_msgs::msg::AgvAutoStatus::SharedPtr msg) {
                WriteStatusToShm(msg);
            });
        
        // 创建定时器读取PLC数据
        io_timer_ = create_wall_timer(
            100ms, std::bind(&PLCBrigde::ReadIOFromShm, this));
    }

    ~PLCBrigde() {
        // 清理写共享内存
        if (shm_struct_write_ != nullptr) {
            munmap(shm_struct_write_, sizeof(WriteToPLCStruct));
        }
        if (shm_fd_write_ != -1) {
            close(shm_fd_write_);
        }
        shm_unlink("CODESYS_MEMORY_WRITE");
        
        // 清理读共享内存
        if (shm_struct_read_ != nullptr) {
            munmap(shm_struct_read_, sizeof(ReadFromPLCStruct));
        }
        if (shm_fd_read_ != -1) {
            close(shm_fd_read_);
        }
        shm_unlink("CODESYS_MEMORY_READ");
        
        RCLCPP_INFO(this->get_logger(), "Dual shared memory resources cleaned up");
    }

private:
    void InitializeSharedMemory() {
        // 初始化写共享内存
        memset(shm_struct_write_, 0, sizeof(WriteToPLCStruct));
        // 初始化读共享内存
        memset(shm_struct_read_, 0, sizeof(ReadFromPLCStruct));
        RCLCPP_INFO(this->get_logger(), "Shared memory structures initialized");
    }

    void WriteEventToShm(const agv_msgs::msg::AGVEvent::SharedPtr msg) {
        // 写入到写共享内存的VehicleEvent
        shm_struct_write_->VehicleEvent.type = msg->type;
        shm_struct_write_->VehicleEvent.action = msg->action;
        shm_struct_write_->VehicleEvent.ackreq = msg->ackreq;
        shm_struct_write_->VehicleEvent.eventtime = msg->eventtime;
        shm_struct_write_->VehicleEvent.vellimit = msg->vellimit;
        shm_struct_write_->VehicleEvent.ecode = msg->ecode;
        shm_struct_write_->VehicleEvent.param1 = msg->param1;
        shm_struct_write_->VehicleEvent.param2 = msg->param2;
        shm_struct_write_->VehicleEvent.param3 = msg->param3;
        shm_struct_write_->VehicleEvent.param4 = msg->param4;
        
        RCLCPP_DEBUG(this->get_logger(), "Event written to WRITE shm: %d", msg->ecode);
    }

    void WriteModeToShm(const agv_msgs::msg::AGVMode::SharedPtr msg) {
        // 写入到写共享内存的VehicleMode
        shm_struct_write_->VehicleMode.mode = msg->mode;
        RCLCPP_DEBUG(this->get_logger(), "Mode written to WRITE shm: %d", msg->mode);
    }

    void WriteStatusToShm(const agv_msgs::msg::AgvAutoStatus::SharedPtr msg) {
        // 写入到写共享内存的VehicleNavigator
        shm_struct_write_->VehicleNavigator.taskid = msg->taskid;
        shm_struct_write_->VehicleNavigator.subtaskid = msg->subtaskid;
        shm_struct_write_->VehicleNavigator.fromnode = msg->fromnode;
        shm_struct_write_->VehicleNavigator.tonode = msg->tonode;
        shm_struct_write_->VehicleNavigator.fprogress = msg->fprogress;
        shm_struct_write_->VehicleNavigator.fprogpercent = msg->fprogpercent;
        shm_struct_write_->VehicleNavigator.curpst_x = msg->curpst_x;
        shm_struct_write_->VehicleNavigator.curpst_y = msg->curpst_y;
        shm_struct_write_->VehicleNavigator.curpst_t = msg->curpst_t;
        shm_struct_write_->VehicleNavigator.curvel_x = msg->curvel_x;
        shm_struct_write_->VehicleNavigator.curvel_z = msg->curvel_z;
        shm_struct_write_->VehicleNavigator.vellevel = msg->vellevel;
        shm_struct_write_->VehicleNavigator.fdevprog = msg->fdevprog;
        shm_struct_write_->VehicleNavigator.fdev = msg->fdev;
        shm_struct_write_->VehicleNavigator.fdevthita = msg->fdevthita;
        shm_struct_write_->VehicleNavigator.fquality = msg->fquality;
        shm_struct_write_->VehicleNavigator.event = msg->event;
        shm_struct_write_->VehicleNavigator.workstate = msg->workstate;
        shm_struct_write_->VehicleNavigator.wheelmovedir = msg->wheelmovedir;
        shm_struct_write_->VehicleNavigator.trajtype = msg->trajtype;
        shm_struct_write_->VehicleNavigator.locatetype = msg->locatetype;
        shm_struct_write_->VehicleNavigator.bits = msg->bits;
        shm_struct_write_->VehicleNavigator.word1 = msg->word1;
        shm_struct_write_->VehicleNavigator.word2 = msg->word2;
        shm_struct_write_->VehicleNavigator.pathword1 = msg->pathword1;
        shm_struct_write_->VehicleNavigator.pathword2 = msg->pathword2;
        shm_struct_write_->VehicleNavigator.rackfollowup = msg->rackfollowup;
        shm_struct_write_->VehicleNavigator.navigationtype = msg->navigationtype;
        shm_struct_write_->VehicleNavigator.driving = msg->driving;
        shm_struct_write_->VehicleNavigator.startdelay = msg->startdelay;
        shm_struct_write_->VehicleNavigator.paused = msg->paused;
        shm_struct_write_->VehicleNavigator.idle = msg->idle;
        shm_struct_write_->VehicleNavigator.orderupdateidnew = msg->orderupdateidnew;
        
        // 处理字符串字段
        SafeStrCopy(shm_struct_write_->VehicleNavigator.scenes, 
                   msg->scenes.c_str(), 
                   sizeof(shm_struct_write_->VehicleNavigator.scenes));
        
        SafeStrCopy(shm_struct_write_->VehicleNavigator.orderidnew, 
                   msg->orderidnew.c_str(), 
                   sizeof(shm_struct_write_->VehicleNavigator.orderidnew));
        
        SafeStrCopy(shm_struct_write_->VehicleNavigator.nodeidnew, 
                   msg->nodeidnew.c_str(), 
                   sizeof(shm_struct_write_->VehicleNavigator.nodeidnew));
        
        RCLCPP_DEBUG(this->get_logger(), "Status written to WRITE shm");
    }

    void ReadIOFromShm() {
        srand((unsigned int)time(NULL));
        shm_struct_write_->IO.digitaloutput0 = (uint8_t)(rand() % 256);
        shm_struct_write_->IO.digitaloutput1 = (uint8_t)(rand() % 256);
        shm_struct_write_->IO.digitaloutput2 = (uint8_t)(rand() % 256);
        shm_struct_write_->IO.digitaloutput3 = (uint8_t)(rand() % 256);
        // 从读共享内存中获取PLC数据
        RCLCPP_INFO(this->get_logger(), "Digital Inputs: %d, %d, %d, %d",
                   shm_struct_read_->IO.digitalinput0,
                   shm_struct_read_->IO.digitalinput1,
                   shm_struct_read_->IO.digitalinput2,
                   shm_struct_read_->IO.digitalinput3);
        
        RCLCPP_INFO(this->get_logger(), "Analog Inputs: %d, %d, %d, %d",
                   shm_struct_read_->IO.AnalogInput0,
                   shm_struct_read_->IO.AnalogInput1,
                   shm_struct_read_->IO.AnalogInput2,
                   shm_struct_read_->IO.AnalogInput3);
        
        // 这里可以添加发布ROS2消息的逻辑
        // 例如：auto io_msg = std::make_unique<agv_msgs::msg::PLCInput>();
        // io_msg->digital_inputs = { ... };
        // io_msg->analog_inputs = { ... };
        // io_pub_->publish(std::move(io_msg));
    }

    // 安全字符串复制函数
    void SafeStrCopy(char* dest, const char* src, size_t dest_size) {
        if (dest_size > 0) {
            strncpy(dest, src, dest_size - 1);
            dest[dest_size - 1] = '\0';  // 确保以null结尾
        }
    }

    // 共享内存相关成员
    int shm_fd_write_ = -1;
    WriteToPLCStruct* shm_struct_write_ = nullptr;
    int shm_fd_read_ = -1;
    ReadFromPLCStruct* shm_struct_read_ = nullptr;
    
    // ROS2 通信成员
    rclcpp::Subscription<agv_msgs::msg::AGVEvent>::SharedPtr event_sub_;
    rclcpp::Subscription<agv_msgs::msg::AGVMode>::SharedPtr mode_sub_;
    rclcpp::Subscription<agv_msgs::msg::AgvAutoStatus>::SharedPtr status_sub_;
    
    rclcpp::TimerBase::SharedPtr io_timer_;
};

int main(int argc, char** argv) {
    rclcpp::init(argc, argv);
    auto node = std::make_shared<PLCBrigde>();
    if (rclcpp::ok()) {
        rclcpp::spin(node);
    }
    rclcpp::shutdown();
    return 0;
}